/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file template.cxx
	\brief class implemenation for template

                    \author Jack Elston
             $Date: 2005/02/12 00:20:45 $
*/

#include <unistd.h>
#include <signal.h>
#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>

#include "joystickThread.h"

  /*<-------Defines--------->*/
  /*<------End Defines------>*/

  /*<---Global Variables---->*/
    extern bool verbose;
		bool joystickDevEnabled;
  /*<-End Global Variables-->*/

JoystickThread::JoystickThread(String name, String dev) : ThreadedPipe(name)
{
	devStr           = dev;
	joystickDevEnabled       = false;

	pad_changed = false;
}

void JoystickThread::openDevice() {
  pad.fd = open(devStr.c_str(), O_RDONLY);
  if (pad.fd > 0) {
    cout << getName() << "::openDevice() - opened " << devStr << endl;

    ioctl(pad.fd, JSIOCGAXES, &pad.axisCount);
    ioctl(pad.fd, JSIOCGBUTTONS, &pad.buttonCount);
    ioctl(pad.fd, JSIOCGVERSION, &pad.version);
    ioctl(pad.fd, JSIOCGNAME(80), &pad.devName);
    fcntl(pad.fd, F_SETFL, O_NONBLOCK);

    cout << "axis : " << (int)pad.axisCount << endl;
    cout << "buttons : " << (int)pad.buttonCount << endl;
    cout << "version : " << pad.version << endl;
    cout << "name : " << pad.devName << endl;

		joystickDevEnabled       = true;
  } else {
		joystickDevEnabled       = false;
	}
}

void JoystickThread::update() 
{

	if( !joystickDevEnabled ) {
		openDevice();
		if( !joystickDevEnabled ) {
			if(verbose)
				cout << getName() << "::openDevice() - failed to open " << devStr << endl;
			sleep(1);
			return;
		}
	} 

  int val, maxFD;
	fd_set readFDs, writeFDs;                   // fd sets
	struct timeval timeout;

  FD_ZERO(&readFDs);	                    		// zero read fd set
  FD_ZERO(&writeFDs);	                    		// zero write fd set
  FD_SET(fifo_FDs[INBOARD_RX],&readFDs);			// add fifo
  FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);			// add fifo

  maxFD = fifo_FDs[INBOARD_RX];
  if(fifo_FDs[INBOARD_TX] > maxFD) maxFD = fifo_FDs[INBOARD_TX];

  joyRead();

	timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;
	val = select(maxFD + 1, &readFDs, &writeFDs, NULL, &timeout);

	if( val < 0 ) {
		if (errno != EINTR)
			cout << getName() << "::update() - select, " <<  strerror(errno) << endl;
	}
	else if(val == 0) {
	}
	/*<----One FD is Active--->*/
	else {

		/*<---TX Operations--->*/
		if(pad_changed && FD_ISSET(fifo_FDs[INBOARD_TX], &writeFDs)) {
			writeFifo(INBOARD_TX);
		  pad_changed = 0;
		}

	}
	usleep(10000);
}

void JoystickThread::joyRead()
{
  int result = read(pad.fd, &pad.ev, sizeof(pad.ev));   // Read the joystick port

  if (result > 0)
  {
    switch (pad.ev.type)
    {
      case JS_EVENT_INIT:
        break;
      case JS_EVENT_AXIS:
      case JS_EVENT_BUTTON:
        memcpy(fifo_msg.data, &pad.ev, sizeof(js_event));
        fifo_msg.size = sizeof(js_event);
				pad_changed = 1;
        break;
      default:
        break;
      }
  } else {
		int errsv = errno;
		switch (errsv) {
			case EAGAIN:
				break;
			case EBADF:
			case ENODEV:
				perror("JoyStickThread::joyRead() - ERROR");
				joystickDevEnabled = false;	
				break;
			default:
				cout << getName() << "joyRead()- nothing read, errno:" << errsv << endl;
		}
	}
}

void JoystickThread::close()
{
}
